#include "StdAfx.h"
#include "ModelMatch.h"
#include <stdlib.h>
#include <time.h>

using namespace std;
using namespace cv;

#define RANDOMTRIES 100 

ModelMatch::ModelMatch(Frame rwFrame, CourtModel model) : _realWorld(rwFrame), 
	_model(model), _lineMask(rwFrame._lineMask)
{
	_score = INT_MIN;
}

ModelMatch::ModelMatch() {

}


void ModelMatch::updateFrame(Frame newFrame){
	_realWorld = newFrame;
}

//Given Four bounding lines of frame to compute Homography
double ModelMatch::fitModelAbsolute(Mat& H, Line h1, Line h2, Line v1, Line v2) {
	
	// find the 4 points intersects the 2 horizontal and 2 vertical lines from the frame.
	vector<Point> rwpts = computeIntersections(h1, h2, v1, v2);
	vector<Point2f> rwpts2f;
	for(size_t i = 0; i < rwpts.size(); i++) {
		Point p = rwpts[i];
		Point2f p2f((float)p.x, (float)p.y);
		rwpts2f.push_back(p2f);
	}

	//find 4 intersection points of the 4 model boudning lines.
	vector<Line> modelLines = _model.findBoundingLines();
	vector<Point> mpts = computeIntersections(modelLines[0], modelLines[1], modelLines[2], modelLines[3]);
	vector<Point2f> mpts2f;
	for(size_t i = 0; i < mpts.size(); i++) {
		Point p = mpts[i];
		Point2f p2f((float)p.x, (float)p.y);
		mpts2f.push_back(p2f);
	}

	//compute homography
	H = findHomography(rwpts2f, mpts2f, CV_RANSAC);

	//score homography.
	Mat currentFrame = _realWorld._scoreMat.clone();
	vector<Line> mappedVerticalLines;
	vector<Line> mappedHorizontalLines;
	drawModelInFrame(currentFrame, H, mappedVerticalLines, mappedHorizontalLines);
	double currentScore = scoreMappedModelLines2(mappedVerticalLines, mappedHorizontalLines);

	return currentScore;
}


//esitmate homography using 2 vertical or 2 horizontal boudning lines.
double ModelMatch::fitModelAbsolute(Mat& H, Line l1, Line l2, bool horizontal) {
	
	//convert the 4 ending point into float points
	vector<Point> rwpts;
	rwpts.push_back(l1.getPoints()[0]);
	rwpts.push_back(l1.getPoints()[1]);
	rwpts.push_back(l2.getPoints()[0]);
	rwpts.push_back(l2.getPoints()[1]);
	vector<Point2f> rwpts2f;
	for(size_t i = 0; i < rwpts.size(); i++) {
		Point p = rwpts[i];
		Point2f p2f((float)p.x, (float)p.y);
		rwpts2f.push_back(p2f);
	}

	//get the corresponding 4 points.
	vector<Point> mpts;
	if(horizontal){
		vector<Line> modelLines = _model.findBoundingLines();
		mpts.push_back(modelLines[0].getPoints()[0]);
		mpts.push_back(modelLines[0].getPoints()[1]);
		mpts.push_back(modelLines[1].getPoints()[0]);
		mpts.push_back(modelLines[1].getPoints()[1]);
	}
	else{
		vector<Line> modelLines = _model.findBoundingLines();
		mpts.push_back(modelLines[2].getPoints()[0]);
		mpts.push_back(modelLines[2].getPoints()[1]);
		mpts.push_back(modelLines[3].getPoints()[0]);
		mpts.push_back(modelLines[3].getPoints()[1]);
	}
	vector<Point2f> mpts2f;
	for(size_t i = 0; i < mpts.size(); i++) {
		Point p = mpts[i];
		Point2f p2f((float)p.x, (float)p.y);
		mpts2f.push_back(p2f);
	}

	//compute the homography
	H = findHomography(rwpts2f, mpts2f, CV_RANSAC);

	//score the homography
	Mat currentFrame = _realWorld._scoreMat.clone();
	vector<Line> mappedVerticalLines;
	vector<Line> mappedHorizontalLines;
	drawModelInFrame(currentFrame, H, mappedVerticalLines, mappedHorizontalLines);
	double currentScore = scoreMappedModelLines2(mappedVerticalLines, mappedHorizontalLines);

	return currentScore;
}




//Compute homography by exaustively looking through all line intersections
double ModelMatch::pointModelExaustiveFit(Mat& H){
	vector<Line> hFrameLines = _realWorld.getHorLines();
	vector<Line> vFrameLines = _realWorld.getVerLines();
	double bestL2 = numeric_limits<double>::max( );

	if(hFrameLines.size()<1 || vFrameLines.size()<1){
		cout<< "returning previous homography" <<endl;
		H = _H.clone();
		return bestL2;
	}

	for(size_t i=0; i<MIN(1, hFrameLines.size()-1); i++){
		Line hFrameLine1 = hFrameLines[i];
		for(size_t j=i+1; j<MIN(1, hFrameLines.size()-1)+1; j++){
			Line hFrameLine2 = hFrameLines[j];
			for(size_t k=0; k<vFrameLines.size()-1; k++){
				Line vFrameLine1 = vFrameLines[k];
				for(size_t l=k+1; l<vFrameLines.size(); l++){
					Line vFrameLine2 = vFrameLines[l];	
					
					vector<Point> frameLineIntersections = computeIntersections(hFrameLine1, hFrameLine2, vFrameLine1, vFrameLine2);
					Mat currentH;
					double currentScore = computeHomographyWithIntersections(frameLineIntersections, currentH);
					if(currentScore < bestL2){
						H = currentH.clone();
						bestL2 = currentScore;
					}
				}
			}
		}
	}
	_score = bestL2;
	if((H.type() == 0 ) && (_H.type()!=0)){
		cout<< "returning previous homography" <<endl;
		H = _H.clone();
		return bestL2;
	}
	
	else{
		_H = H.clone();
		return bestL2;
	}
}



//Compute homography using 4 intersections from frame line intersections.
//See "FAST CAMERA CALIBRATION FOR THE ANALYSIS OF SPORT SEQUENCES"
double ModelMatch::computeHomographyWithIntersections(vector<Point> frameIntersections, Mat& H){
	
	vector<Line> boundingLines = _model.findBoundingLines();
	vector<Line> hModelLines = _model.getHorLines();
	vector<Line> vModelLines = _model.getVerLines();
	double bestL2 = numeric_limits<double>::max( );
	
	//convert the points into float points
	vector<Point2f> frameIntersections2f;
	for(size_t i = 0; i < frameIntersections.size(); i++) {
		Point p = frameIntersections[i];
		Point2f p2f((float)p.x, (float)p.y);
		frameIntersections2f.push_back(p2f);
	}
	
	
	//try 2 different horizontal lines and 2 different vertical lines at a time.
	for(size_t i=0; i<hModelLines.size()-1; i++){
		Line hModelLine1 = hModelLines[i];
		for(size_t j=i+1; j<hModelLines.size(); j++){
			Line hModelLine2 = hModelLines[j];
			
			for(size_t k=0; k<vModelLines.size()-1; k++){
				Line vModelLine1 = vModelLines[k];	
				for(size_t l=k+1; l<vModelLines.size(); l++){
					Line vModelLine2 = vModelLines[l];	

					//compute the 4 model lines intersections and convert them into float points
					vector<Point> modelIntersections = computeIntersections(hModelLine1, hModelLine2, vModelLine1, vModelLine2);
					vector<Point2f> modelIntersections2f;
					for(size_t i = 0; i < modelIntersections.size(); i++) {
						Point p = modelIntersections[i];
						Point2f p2f((float)p.x, (float)p.y);
						modelIntersections2f.push_back(p2f);
					}

					//compute the homograpy
					Mat currentH = findHomography(frameIntersections2f, modelIntersections2f, 0);
					//Mat currentH = computeHomography(frameIntersections, modelIntersections);
					
					//using the computed homography project the model lines into frame.
					Point minHorizontalPoint1 = boundingLines[0].getPoints()[0];
					Point minHorizontalPoint2 = boundingLines[0].getPoints()[1];
					Point maxHorizontalPoint1 = boundingLines[1].getPoints()[0];

					Point estimatedMinHP1 = translatePPrimetoP(minHorizontalPoint1, currentH);
					Point estimatedMinHP2 = translatePPrimetoP(minHorizontalPoint2, currentH);
					Point estimatedMaxHP1 = translatePPrimetoP(maxHorizontalPoint1, currentH);
					//comput the proejcted court area and aspect ratio
					float estimatedAspectRatio = ((float)abs(estimatedMaxHP1.y - estimatedMinHP1.y)) / ((float)abs(estimatedMinHP2.x - estimatedMinHP1.x));
					float estimatedArea = (estimatedMaxHP1.y - estimatedMinHP1.y) * (estimatedMinHP2.x - estimatedMinHP1.x);
					
					//homography with outrageous projected court area and aspect ratio are rejected.
					if(!(estimatedAspectRatio > _model.courtAspectRatio || estimatedAspectRatio < 0.25 * _model.courtAspectRatio || 
						estimatedArea < _model.courtArea || estimatedArea > 2*(_realWorld._cvFrame.cols * _realWorld._cvFrame.rows) ) ){
						
						//double horizontalScore = scoreHomographyL2HorizontalLines(currentH);
						//double verticalScore = scoreHomographyL2VerticalLines(currentH);
						//double currentScore = horizontalScore + verticalScore;
						
						//score the homopgraphy.
						Mat currentFrame = _realWorld._scoreMat.clone();
						vector<Line> mappedVerticalLines;
						vector<Line> mappedHorizontalLines;
						drawModelInFrame(currentFrame, currentH, mappedVerticalLines, mappedHorizontalLines);
						//double currentScore = scoreMappedModelLines(mappedVerticalLines, mappedHorizontalLines);
						double currentScore = scoreMappedModelLines2(mappedVerticalLines, mappedHorizontalLines);

						if(SCORE_DEBUG && _score > -1200){
							cout<< currentScore <<endl;
							imshow("Debug",currentFrame);
							cvWaitKey(0);
						}

						//if homography scores better than the best so far, then update.
						if(currentScore < bestL2){
							bestL2 = currentScore;
							H = currentH.clone();
						}
					}
				}
			}
		}
	}
	return bestL2;
}

//NOT USED.
double ModelMatch::scoreMappedModelLines(vector<Line> verticalLines, vector<Line> horizontalLines){
	double score =0;
	Line yAxis (Point(0, 0), Point(0, 1000));
	for(int i=0; i<horizontalLines.size(); i++){
		Point p1 = horizontalLines[i].getPoints()[0];
		Point p2 = horizontalLines[i].getPoints()[1];
		double slope = ((p1.y-p2.y)*1.0)/((p1.x-p2.x)*1.0);
		int xStart, xEnd, yIntercept;
		Point intersection = horizontalLines[i].findIntersection(yAxis);
		yIntercept = intersection.y;
		if(p1.x>p2.x){
			xStart = p2.x;
			xEnd = p1.x;
		}
		else{
			xStart = p1.x;
			xEnd = p2.x;
		}

		for(int x=xStart; x<xEnd; x++){
			int y = slope*x+yIntercept;
			if(x>0 && x<_realWorld._cvFrame.cols && y>0 && y<_realWorld._cvFrame.rows){
				if(goodMatch(x, y)){
					score--;
				}
				else{
					score+=0.5;
				}
			}
		}
	}

	for(int i=0; i<verticalLines.size(); i++){
		Point p1 = verticalLines[i].getPoints()[0];
		Point p2 = verticalLines[i].getPoints()[1];
		double slope = ((p1.y-p2.y)*1.0)/((p1.x-p2.x)*1.0);
		int xStart, xEnd, yIntercept;
		Point intersection = horizontalLines[i].findIntersection(yAxis);
		yIntercept = intersection.y;
		if(p1.x>p2.x){
			xStart = p2.x;
			xEnd = p1.x;
		}
		else{
			xStart = p1.x;
			xEnd = p2.x;
		}
		for(int x=xStart; x<xEnd; x++){
			int y = slope*x+yIntercept;
			if(x>0 && x<_realWorld._cvFrame.cols && y>0 && y<_realWorld._cvFrame.rows){
				if(goodMatch(x, y)){
					score--;
				}
				else{
					score+=0.5;
				}
			}
		}
	}
	return score;
}


//Used to score homography by projecting court model lines onto frame see if they overlap with the white detected lines.
double ModelMatch::scoreMappedModelLines2(vector<Line> verticalLines, vector<Line> horizontalLines){
	
	double score =0;
	double totalXScore =0;
	//scoreing horizontal lines
	for(int i=0; i<horizontalLines.size(); i++){
		Point p1 = horizontalLines[i].getPoints()[0];
		Point p2 = horizontalLines[i].getPoints()[1];
		int xStart, xEnd;
		if(p1.x>p2.x){
			xStart = p2.x;
			xEnd = p1.x;
		}
		else{
			xStart = p1.x;
			xEnd = p2.x;
		}
		double normalizer = xEnd-xStart;
		double xScore =0;
		//simple speed up by skpping 5 pixels at a time
		for(int x=xStart; x<xEnd; x+=5){
			//compute location y based on x.
			float y = ((p1.y-p2.y) * (x - p2.x)*1.0) / (p1.x - p2.x) + p2.y;
			if(goodMatch(x, y)){
				xScore--;
			}
			else{
				xScore+=0;
			}
		}
		score += xScore;
		totalXScore +=xScore;
	}

	//scoring vertical lines. Same approach as horizontal lines.
	double totalYScore =0;
	for(int i=0; i<verticalLines.size(); i++){
		Point p1 = verticalLines[i].getPoints()[0];
		Point p2 = verticalLines[i].getPoints()[1];
		int yStart, yEnd;
		if(p1.y>p2.y){
			yStart = p2.y;
			yEnd = p1.y;
		}
		else{
			yStart = p1.y;
			yEnd = p2.y;
		}
		double normalizer = yEnd - yStart;
		double yScore = 0; 
		for(int y=yStart; y<yEnd; y+=5){
			float x = ((p1.x-p2.x) * (y - p2.y)*1.0) / (p1.y - p2.y) + p2.x;
			if(goodMatch(x, y)){
				yScore--;
			}
			else{
				yScore+=0;
			}
		}
		score += yScore;
		totalYScore += yScore;
	}

	return score;
}


//check if the window around (col, row) in the frame's scoreMat has white pixels
bool ModelMatch::goodMatch(int col, int row){
	Mat lineMask = _realWorld._scoreMat;
	int xWindowSize = 5;
	int yWindowSize = 5;

	for(int r=-yWindowSize; r<=yWindowSize; r++){
		for(int c=-xWindowSize; c<=xWindowSize; c++){
			if((col+c)>0 && (col+c)<_realWorld._cvFrame.cols && (r+row)>0 && (r+row)<_realWorld._cvFrame.rows){
				uchar value = lineMask.at<uchar>((row+r),(col+c));
				if(value>0){
					return true;
				}
			}
		}
	}
	return false;
}



//NOT USED. 
double ModelMatch::modelExaustiveFit(Mat& H, vector<Line> modelLines, vector<Line> frameLines, bool segments){
	vector<Line> boundingLines = _model.findBoundingLines();

	vector<Point2f> f2FPoints(4);
	vector<Point2f> m2FPoints(4);

	vector<Point> fPoints(4);
	vector<Point> mPoints(4);

	Mat currentH;
	double bestL2 = numeric_limits<double>::max( );

	//search through model and frame corresponding lines to compute H
	for(size_t fFirstLineNum = 0; fFirstLineNum<frameLines.size()-1; fFirstLineNum++){
		vector<Point> fFirstPts = frameLines[fFirstLineNum].getPoints();
		fPoints[0] = fFirstPts[0];
		fPoints[1] = fFirstPts[1];

		Point2f fFirstPt1 ((float)fFirstPts[0].x, (float)fFirstPts[0].y);
		Point2f fFirstPt2 ((float)fFirstPts[fFirstPts.size()-1].x, (float)fFirstPts[fFirstPts.size()-1].y);
		f2FPoints[0] = fFirstPt1;
		f2FPoints[1] = fFirstPt2;

		for(size_t fSecondLineNum = (fFirstLineNum+1); fSecondLineNum<frameLines.size(); fSecondLineNum++){
			vector<Point> fSecondPts = frameLines[fSecondLineNum].getPoints();
			fPoints[2] = fSecondPts[0];
			fPoints[3] = fSecondPts[1];
			
			Point2f fSecondPt1 ((float)fSecondPts[0].x,(float)fSecondPts[0].y);
			Point2f fSecondPt2  ((float)fSecondPts[fSecondPts.size()-1].x, (float)fSecondPts[fSecondPts.size()-1].y);
			f2FPoints[2] = fSecondPt1;
			f2FPoints[3] = fSecondPt2;

			if((fFirstPt1.x == fSecondPt1.x && fFirstPt1.y == fSecondPt1.y) || (fFirstPt1.x == fSecondPt2.x && fFirstPt1.y == fSecondPt2.y)||
				(fFirstPt2.x == fSecondPt1.x && fFirstPt2.y == fSecondPt1.y) || (fFirstPt2.x == fSecondPt2.x && fFirstPt2.y == fSecondPt2.y)){
				continue;
			}

			for(size_t mFirstLineNum = 0; mFirstLineNum<modelLines.size()-1; mFirstLineNum++){
				vector<Point> mFirstPts = modelLines[mFirstLineNum].getPoints();
				mPoints[0] = mFirstPts[0];
				mPoints[1] = mFirstPts[1];

				Point mFirstPt1 ((float)mFirstPts[0].x, (float)mFirstPts[0].y);
				Point mFirstPt2 ((float)mFirstPts[mFirstPts.size()-1].x, (float)mFirstPts[mFirstPts.size()-1].y);
				m2FPoints[0] = mFirstPt1;
				m2FPoints[1] = mFirstPt2;       

				for(size_t mSecondLineNum = (mFirstLineNum+1); mSecondLineNum<modelLines.size(); mSecondLineNum++){
					vector<Point> mSecondPts = modelLines[mSecondLineNum].getPoints();
					mPoints[2] = mSecondPts[0];
					mPoints[3] = mSecondPts[1];
					
					Point mSecondPt1 ((float)mSecondPts[0].x, (float)mSecondPts[0].y);
					Point mSecondPt2 ((float)mSecondPts[mSecondPts.size()-1].x, (float)mSecondPts[mSecondPts.size()-1].y);
					m2FPoints[2] = mSecondPt1;
					m2FPoints[3] = mSecondPt2;
					
					if((mFirstPt1.x == mSecondPt1.x && mFirstPt1.y == fSecondPt1.y) || (mFirstPt1.x == mSecondPt2.x && mFirstPt1.y == mSecondPt2.y)||
					(mFirstPt2.x == mSecondPt1.x && mFirstPt2.y == mSecondPt1.y) || (mFirstPt2.x == mSecondPt2.x && mFirstPt2.y == mSecondPt2.y)){
						continue;
					}


					//Solve for homography here
					currentH = findHomography(f2FPoints, m2FPoints, RANSAC);
					//currentH = computeHomography(fPoints, mPoints);


					Point minHorizontalPoint1 = boundingLines[0].getPoints()[0];
					Point minHorizontalPoint2 = boundingLines[0].getPoints()[1];

					Point maxHorizontalPoint1 = boundingLines[1].getPoints()[0];

					Point estimatedMinHP1 = translatePPrimetoP(minHorizontalPoint1, currentH);
					Point estimatedMinHP2 = translatePPrimetoP(minHorizontalPoint2, currentH);
					Point estimatedMaxHP1 = translatePPrimetoP(maxHorizontalPoint1, currentH);

					//check angles of projected lines. Assuming projected horizontal lines should also be horizontal and projected vertical lines are still vertical.
					if(checkProjectedLines(currentH)) continue;

					float estimatedAspectRatio = ((float)abs(estimatedMaxHP1.y - estimatedMinHP1.y)) / ((float)abs(estimatedMinHP2.x - estimatedMinHP1.x));

					float estimatedArea = (estimatedMaxHP1.y - estimatedMinHP1.y) * (estimatedMinHP2.x - estimatedMinHP1.x);
					
					if(!(estimatedAspectRatio > _model.courtAspectRatio || estimatedAspectRatio < 0.25 * _model.courtAspectRatio || 
						estimatedArea < _model.courtArea || estimatedArea > 2*(_realWorld._cvFrame.cols * _realWorld._cvFrame.rows) ) ){

						double currentScore;
						//if(!segments){
						//	double horizontalScore = scoreHomographyL2HorizontalLines(currentH, true);
						//	double verticalScore = scoreHomographyL2VerticalLines(currentH, true);
						//	currentScore = horizontalScore + verticalScore;
						//}
						//else{
							Mat currentFrame = _realWorld._scoreMat.clone();
							vector<Line> mappedVerticalLines;
							vector<Line> mappedHorizontalLines;
							drawModelInFrame(currentFrame, currentH, mappedVerticalLines, mappedHorizontalLines);
							currentScore = scoreMappedModelLines2(mappedVerticalLines, mappedHorizontalLines);
							
							if(DEBUG){
								cout<<currentScore<<endl;
								//imshow("Debug", currentFrame);
								//cvWaitKey(0);
							}
						//}


						if(currentScore < bestL2){
							//cout << "Best Score: " <<  currentScore << " Old Score: " << bestL2 << endl;
							bestL2 = currentScore;
							H = currentH.clone();
						}
					}
				}
			}
		}
	}
	cout<<endl;
	return bestL2;
}



//refer to "FAST CAMERA CALIBRATION FOR THE ANALYSIS OF SPORT SEQUENCES" 
double ModelMatch::fastModelExaustiveFit(Mat& H){
	
	if(DEBUG){
		cvNamedWindow("Debug", CV_WINDOW_AUTOSIZE);
		imshow("Debug", _realWorld._scoreMat);
		//imwrite("score.png", _realWorld._scoreMat);
		cvWaitKey(0);
	}
	
	
	vector<Line> hFrameLines = _realWorld.getHorLines();
	vector<Line> vFrameLines = _realWorld.getVerLines();
	if(vFrameLines.size()<1 || hFrameLines.size()<1){
		cout<< "returning previous homography" <<endl;
		H = _H.clone();
		return 0;
	}

	if(hFrameLines.size()>4){

		vector<Line> vModelLines = _model.getVerLines();
		double score = modelExaustiveFit(H, vModelLines, vFrameLines, false);
		
		if(!(H.type() == 0 || score> 800)){
		//if(! (H.type() == 0)){
			_H = H.clone();
			return score;
		}
	}

	if(vFrameLines.size()>4){

		vector<Line> hModelLines = _model.getHorLines();
		double score = modelExaustiveFit(H, hModelLines, hFrameLines, false);
		
		//if(!(H.type() == 0 || score> 800)){
		if(!(H.type() == 0)){
			_H = H.clone();
			return score;
		}
		else{
			cout<< "returning previous homography" <<endl;
			H = _H.clone();
			return score;
		}
	}

	double score ;
	//if(vFrameLines.size()<4 || hFrameLines.size()<4){
		vector<Line> vModelLines = _model.getSegmentVerLines();
		for(int i=0; i < _realWorld.getSegmentVerLines().size(); i++){
			vFrameLines.push_back(_realWorld.getSegmentVerLines()[i]);
		}
		score = modelExaustiveFit(H, vModelLines, vFrameLines, true);
	//}
	
	if(!(H.type() == 0 || score> 800)){
	//if(!(H.type() == 0)){
		_H = H.clone();
		return score;
	}
	else{
		cout<< "returning previous homography" <<endl;
		H = _H.clone();
		return score;
	}
	
}


//Used by other methods to reject homographies based on the assumption a horizontal line in model should be horizontal after projection. Same assumption for vertical lines.
bool ModelMatch::checkProjectedLines(Mat H){
	vector<Line> hModelLines = _model.getHorLines();
	for(int i=0; i<hModelLines.size(); i++){
		Point p1 = hModelLines[i].getPoints()[0];
		Point p2 = hModelLines[i].getPoints()[1];

		Point projectedP1 = translatePPrimetoP(p1, H);
		Point projectedP2 = translatePPrimetoP(p2, H);

		Line projectedLine(projectedP1, projectedP2);
		if(!projectedLine.isHorizontal()){
			return true;
		}
	}

	vector<Line> vModelLines = _model.getVerLines();
	for(int i=0; i<vModelLines.size(); i++){
		Point p1 = vModelLines[i].getPoints()[0];
		Point p2 = vModelLines[i].getPoints()[1];

		Point projectedP1 = translatePPrimetoP(p1, H);
		Point projectedP2 = translatePPrimetoP(p2, H);

		Line projectedLine(projectedP1, projectedP2);
		if(!projectedLine.isVertical()){
			return true;
		}
	}

	return false;
}


//Score homography using L2 distance of projected model horizontal intersections and detected horizontal intersections.
//The score is symmetric, so it alos accounts for L2 distance of projected detected intersections and the model intesections.
double ModelMatch::scoreHomographyL2HorizontalLines(Mat H, bool segments){
	if(H.type() == 0){
		cout<< "scoreHomographyL2HorizontalLines" <<endl;
	}
	vector<Line> frameHorizontalLines, modelHorizontalLines;
	if(segments){
		frameHorizontalLines = _realWorld.getSegmentHorLines();
		modelHorizontalLines = _model.getSegmentHorLines();
	}
	else{
		frameHorizontalLines = _realWorld.getHorLines();
		modelHorizontalLines = _model.getHorLines();
	}

	Mat matchScores = Mat::ones(frameHorizontalLines.size(), modelHorizontalLines.size(), CV_64F) * INT_MAX;
	//Computer L2 score for every  frame line vs. all model lines
	for(size_t frameLineNum=0; frameLineNum<frameHorizontalLines.size(); frameLineNum++){
		for (size_t modelLineNum=0; modelLineNum<modelHorizontalLines.size(); modelLineNum++){
			double score = scoreHomographySymmetricL2(frameHorizontalLines[frameLineNum].getPoints(), modelHorizontalLines[modelLineNum].getPoints(), H);
			if(score < 0){
				cout<< score << endl;
			}
			matchScores.at<double>(frameLineNum, modelLineNum) = score;
		}
	}
	double accumulateScore =0;
	//find best score and set it to the maximum then find the next best score
	for(size_t frameLineNum=0; frameLineNum< MIN(frameHorizontalLines.size(), modelHorizontalLines.size()); frameLineNum++){
		double min, max;
		Point minLoc, maxLoc;
		minMaxLoc(matchScores, &min, &max, &minLoc, &maxLoc, Mat());


		accumulateScore += min;

		for(int i=0; i<matchScores.rows; i++){
			matchScores.at<double>(i, minLoc.x) = INT_MAX;
		}
		for(int i=0; i<matchScores.cols; i++){
			matchScores.at<double>(minLoc.y, i) = INT_MAX;
		}
	}

	if(modelHorizontalLines.size() > frameHorizontalLines.size()){
		accumulateScore += (modelHorizontalLines.size() - frameHorizontalLines.size())*400;
	}
	return accumulateScore/frameHorizontalLines.size();;
}

//Score homography using L2 distance of projected model vertical intersections and detected vertical intersections.
//The score is symmetric, so it alos accounts for L2 distance of projected detected intersections and the model intesections.
double ModelMatch::scoreHomographyL2VerticalLines(Mat H, bool segments){
	if(H.type() == 0){
		cout<< "scoreHomographyL2VerticalLines" <<endl;
	}

	vector<Line> frameVerticalLines, modelVerticalLines;
	if(segments){
		frameVerticalLines = _realWorld.getSegmentVerLines();
		modelVerticalLines = _model.getSegmentVerLines();
	}
	else{
		frameVerticalLines = _realWorld.getVerLines();
		modelVerticalLines = _model.getVerLines();
	}

	Mat matchScores = Mat::ones(frameVerticalLines.size(), modelVerticalLines.size(), CV_64F) * INT_MAX;
	//Computer L2 score for every  frame line vs. all model lines
	for(size_t frameLineNum=0; frameLineNum<frameVerticalLines.size(); frameLineNum++){
		for (size_t modelLineNum=0; modelLineNum<modelVerticalLines.size(); modelLineNum++){
			//double score = scoreHomographyL2(frameVerticalLines[frameLineNum].getPoints(), modelVerticalLines[modelLineNum].getPoints(), H);
			double score = scoreHomographySymmetricL2(frameVerticalLines[frameLineNum].getPoints(), modelVerticalLines[modelLineNum].getPoints(), H);
			if(score < 0){
				cout<< score << endl;
			}
			matchScores.at<double>(frameLineNum, modelLineNum) = score; 
		}
	}
	double accumulateScore =0;
	//find best matches.
	for(size_t frameLineNum=0; frameLineNum< MIN(frameVerticalLines.size(),modelVerticalLines.size()); frameLineNum++){
		double min, max;
		Point minLoc, maxLoc;
		minMaxLoc(matchScores, &min, &max, &minLoc, &maxLoc, Mat());
		accumulateScore += min;

		for(int i=0; i<matchScores.rows; i++){
			matchScores.at<double>(i, minLoc.x) = INT_MAX;
		}
		for(int i=0; i<matchScores.cols; i++){
			matchScores.at<double>(minLoc.y, i) = INT_MAX;
		}
		//cout << "Row : " <<  minLoc.y << " Col: " << minLoc.x << endl;
	}
	//cout << "Score : " <<  accumulateScore << endl;

	if(modelVerticalLines.size() > frameVerticalLines.size()){
		accumulateScore += (modelVerticalLines.size() - frameVerticalLines.size())*400;
	}
	return accumulateScore/frameVerticalLines.size();
}

//Compute L2 distance between points in rwPoints and model points. Measure is symmetric.
double ModelMatch::scoreHomographySymmetricL2(vector<Point> rwPoints, vector<Point> modelPts, Mat H){
	double score = 0;
	for(size_t i = 0; i < rwPoints.size(); i++) {
		Point2f p = rwPoints[i];
		Point2f estimatedMPt = translatePtoPPrime(p, H);
		double xDif = modelPts[i].x - estimatedMPt.x;
		double yDif = modelPts[i].y - estimatedMPt.y;
		double l2Distance = sqrt(pow(xDif, 2.0)+pow(yDif, 2.0));
		score += l2Distance;

		p = modelPts[i];
		estimatedMPt = translatePPrimetoP(p, H);
		xDif = rwPoints[i].x - estimatedMPt.x;
		yDif = rwPoints[i].y - estimatedMPt.y;
		l2Distance = sqrt(pow(xDif, 2.0)+pow(yDif, 2.0));
		score += l2Distance;

	}
	return score;
}

//Compute L2 distance between points in rwPoints and model points.
double ModelMatch::scoreHomographyL2(vector<Point2f> rwPoints, vector<Point2f> modelPts, Mat H){
	double score = 0;
	for(size_t i = 0; i < rwPoints.size(); i++) {
		Point2f p = rwPoints[i];
		Point2f estimatedMPt = translatePtoPPrime(p, H);
		double xDif = modelPts[i].x - estimatedMPt.x;
		double yDif = modelPts[i].y - estimatedMPt.y;
		double l2Distance = sqrt(pow(xDif, 2.0)+pow(yDif, 2.0));
		score += l2Distance;
	}
	return score;
}

//Compute L2 distance between points in rwPoints and model points.
double ModelMatch::scoreHomographyL2(vector<Point> rwPoints, vector<Point> modelPts, Mat H){
	double score = 0;
	for(size_t i = 0; i < rwPoints.size(); i++) {
		Point2f p = rwPoints[i];
		Point2f estimatedMPt = translatePtoPPrime(p, H);
		double xDif = modelPts[i].x - estimatedMPt.x;
		double yDif = modelPts[i].y - estimatedMPt.y;
		double l2Distance = sqrt(pow(xDif, 2.0)+pow(yDif, 2.0));
		score += l2Distance;
	}
	return score; //((double)score / rwPoints.size());
}

//Tries random model fit with default 100 tries;
double ModelMatch::fitModelWithRandomTries() {
	return fitModelWithRandomTries(RANDOMTRIES);
}

//Randomly selects RW lines and exaustively tries all model lines
//to find best fit in numTries tries.
double ModelMatch::fitModelExaustiveWithRandomTries(int numTries) {
	double bestScore = INT_MAX;

	vector<Line> rwHLines = _realWorld.getHorLines();
	vector<Line> rwVLines = _realWorld.getVerLines();
	vector<Intersection> rwAllIntSecs = _realWorld.getIntSecs();

	for(int t = 0; t < numTries; t++) {

		vector<Line> rwHs = chooseLinePair(rwHLines);
		vector<Line> rwVs = chooseLinePair(rwVLines);
		vector<Intersection> rwIntSecs;
		vector<Point> rwIntSecPts = computeIntersections(rwHs[0], rwHs[1], rwVs[0], rwVs[1], rwIntSecs);

		double score = fitModelExaustive(rwHs[0], rwHs[1], rwVs[0], rwVs[1]);
		cout << "Score: " << score << endl;
		if(score < bestScore) {
			bestScore = score;
			_score = score;
		}
	}

	return bestScore;
}

//Randomly selects RW and Model lines and does numTries
//iterations to fit the model.
double ModelMatch::fitModelWithRandomTries(int numTries) {
	double bestScore = INT_MAX;

	vector<Line> boundingLines = _realWorld.findBoundingLines();
	vector<Line> modelLines = _model.allLines;

	vector<Line> rwHLines = _realWorld.getHorLines();
	vector<Line> rwVLines = _realWorld.getVerLines();
	vector<Intersection> rwAllIntSecs = _realWorld.getIntSecs();

	vector<Line> mHLines = _model.getHorLines();
	vector<Line> mVLines = _model.getVerLines();
	vector<Intersection> mAllIntSecs = _model._intersections;

	for(int t = 0; t < numTries; t++) {

		vector<Line> rwHs = chooseLinePair(rwHLines);
		vector<Line> rwVs = chooseLinePair(rwVLines);
		vector<Intersection> rwIntSecs;
		vector<Point> rwIntSecPts = computeIntersections(rwHs[0], rwHs[1], rwVs[0], rwVs[1], rwIntSecs);

		vector<Line> mHs = chooseLinePair(mHLines);
		vector<Line> mVs = chooseLinePair(mVLines);
		vector<Intersection> mIntSecs;
		vector<Point> mIntSecPts = computeIntersections(mHs[0], mHs[1], mVs[0], mVs[1], mIntSecs);

		Mat H = computeHomography(rwIntSecPts, mIntSecPts);
		double score = scoreHomography(rwAllIntSecs, mAllIntSecs, H);
		bool inRectBounds = this->withinBounds(boundingLines, modelLines, H);
		bool inBounds = this->withinFrame(_realWorld, modelLines, H);
		//double score = scoreHomographyWithLineFilling(_lineMask, H, _model.allLines);
		cout << "Score: " << score << endl;
		double B = getB(H);
		//cout << "B: " << B << endl;
		if( 0.5 <= B && B <= 4) {
			cout << "OMG IT HIT IT" << endl;
			cout << "B: " << B << endl;
		}

		if(score < bestScore && inBounds && inRectBounds) {
			bestScore = score;
			_score = bestScore;
			_H = H;

			_rwHPair = rwHs;
			_rwVPair = rwVs;
			_mHPair = mHs;
			_mVPair = mVs;

			_rwIntSecs = rwIntSecs;
			_mIntSecs = mIntSecs;
		}       
	}

	return bestScore;
}

//NOT USED
//Given 4 real world lines find best model fit.
double ModelMatch::fitModelExaustive(Line h1, Line h2, Line v1, Line v2) {
	double score = INT_MAX;
	_mHPair = vector<Line>(2);
	_mVPair = vector<Line>(2);

	vector<Line> rwHLines = _realWorld.getHorLines();
	vector<Line> rwVLines = _realWorld.getVerLines();
	vector<Intersection> rwInts = _realWorld.getIntSecs();

	_rwHPair = vector<Line>(2);
	_rwVPair = vector<Line>(2);
	_rwHPair[0] = h1;
	_rwHPair[1] = h2;
	_rwVPair[0] = v1;
	_rwVPair[1] = v2;

	vector<Point> rwIntSecPts = computeIntersections(h1, h2, v1, v2);

	vector<Line> bounds = _realWorld.findBoundingLines();

	vector<Line> mHLines = _model.getHorLines();
	vector<Line> mVLines = _model.getVerLines();

	for(size_t i = 0; i < mHLines.size()-1; i++) {
		Line h1 = mHLines[i];
		for(size_t j = i+1; j < mHLines.size(); j++) {
			Line h2 = mHLines[j];
			for(size_t k = 0; k < mVLines.size()-1; k++) {
				Line v1 = mVLines[k];
				for(size_t l = k+1; l < mVLines.size(); l++) {
					Line v2 = mVLines[l];

					vector<Intersection> modelIntSecs;
					vector<Point> modelIntSecPts = computeIntersections(h1, h2, v1, v2, modelIntSecs);

					Mat homography = computeHomography(rwIntSecPts, modelIntSecPts);

					double testScore = scoreHomography(rwInts, modelIntSecs, homography);

					bool isInBounds = this->withinBounds(bounds, _model.allLines, homography);
					bool isInFrame = this->withinFrame(_realWorld, _model.allLines, homography);

					if(testScore < score) {
						score = testScore;
					}
				}
			}
		}
	}
	return score;
}

//Randomly selects 4 lines from RW and exaustively finds best model fitting from all
//model pairs
double ModelMatch::fitModelExaustive() {
	double score = INT_MAX;
	_mHPair = vector<Line>(2);
	_mVPair = vector<Line>(2);

	vector<Line> rwHLines = _realWorld.getHorLines();
	vector<Line> rwVLines = _realWorld.getVerLines();
	vector<Intersection> rwInts = _realWorld.getIntSecs();

	vector<Line> hPair = chooseLinePair(rwHLines);
	vector<Line> vPair = chooseLinePair(rwVLines);
	_rwHPair = hPair;
	_rwVPair = vPair;
	vector<Intersection> rwIntSecs;
	vector<Point> rwIntSecPts = computeIntersections(hPair[0], hPair[1], vPair[0], vPair[1], rwIntSecs);

	vector<Line> mHLines = _model.getHorLines();
	vector<Line> mVLines = _model.getVerLines();
	vector<Intersection> mIntSecs = _model._intersections;

	for(size_t i = 0; i < mHLines.size()-1; i++) {
		Line h1 = mHLines[i];
		for(size_t j = i+1; j < mHLines.size(); j++) {
			Line h2 = mHLines[j];
			for(size_t k = 0; k < mVLines.size()-1; k++) {
				Line v1 = mVLines[k];
				for(size_t l = k+1; l < mVLines.size(); l++) {
					Line v2 = mVLines[l];

					vector<Intersection> modelIntSecs;
					vector<Point> modelIntSecPts = computeIntersections(h1, h2, v1, v2, modelIntSecs);

					Mat homography = computeHomography(rwIntSecPts, modelIntSecPts);

					//double testScore = scoreHomography(rwIntSecPts, modelIntSecPts, homography);
					double testScore = scoreHomographyL2(rwIntSecPts, modelIntSecPts, homography);

					if(testScore < score) {
						score = testScore;

						_mHPair[0] = h1;
						_mHPair[1] = h2;

						_mVPair[0] = v1;
						_mVPair[1] = v2;

						_H = homography;
						_mIntSecs = modelIntSecs;
						_rwIntSecs = rwIntSecs;
					}
				}
			}
		}
	}
	return score;
}
bool ModelMatch::withinBounds(vector<Line> bounds, vector<Line> modelLines, Mat H) {

	Line l1 = bounds[0];    //minhor
	Line l2 = bounds[1];    //maxHor
	Line l3 = bounds[2];    //minVer
	Line l4 = bounds[3];    //maxVer

	vector<Point> intSecPts = this->computeIntersections(l1, l2, l3, l4);

	intSecPts[0].x -= 20;
	intSecPts[0].y -= 20;

	intSecPts[1].x += 20;
	intSecPts[1].y -= 20;

	intSecPts[2].x -= 60;
	intSecPts[2].y += 40;

	intSecPts[3].x += 60;
	intSecPts[3].y += 40;

	Line newLine;
	for(int i = 0; i < modelLines.size(); i++) {
		Line& prevLine = modelLines[i];
		translateLine(H, newLine, prevLine);
		if(!withinBoundsRect(intSecPts, newLine)) {
			return false;
		}
	}
	return true;
}

//NOT USED
bool ModelMatch::withinFrame(Frame frame, vector<Line> lines, Mat H) {
	Mat fMat = frame._cvFrame;
	int height = fMat.rows;
	int width = fMat.cols;

	Line newLine;
	for(int i = 0; i < lines.size(); i++) {
		Line& prevLine = lines[i];
		translateLine(H, newLine, prevLine);
		vector<Point> points = newLine.getPoints();
		Point p1 = points[0];
		Point p2 = points[1];

		bool p1InFrame = p1.x >= 0 && p1.x <= width && p1.y >= 0 && p1.y <= height;
		bool p2InFrame = p2.x >= 0 && p2.x <= width && p2.y >= 0 && p2.y <= height;

		if(p1InFrame && p2InFrame) {
			continue;
		}
		else {
			return false;
		}
	}
	return true;
}

//NOT USED
bool ModelMatch::withinBoundsRect(vector<Point> bounds, Line line) {
	Point A = bounds[0];
	Point B = bounds[1];
	Point C = bounds[2];
	Point D = bounds[3];

	vector<Point> linePts = line.getPoints();

	bool result = withinBoundsTriangle(A, B, C, linePts[0]) &&
		withinBoundsTriangle(B, C, D, linePts[0]) &&
		withinBoundsTriangle(A, B, C, linePts[1]) &&
		withinBoundsTriangle(B, C, D, linePts[1]);

	return result;
}

//NOT USED
bool ModelMatch::withinBoundsTriangle(Point a, Point b, Point c, Point p) {     

	bool result = sameSide(p, a, b, c) && sameSide(p, b, a, c) && 
		sameSide(p, c, a, b);

	return result;
}
//NOT USED
bool ModelMatch::sameSide(Point p, Point p2, Point a, Point b) {
	Mat ba = Mat::zeros(1,3,CV_64F);
	ba.at<double>(0,0) = (double)(b.x-a.x);
	ba.at<double>(0,1) = (double)(b.y-a.y);
	ba.at<double>(0,2) = 1.0f;

	Mat p1a = Mat::zeros(1,3,CV_64F);
	p1a.at<double>(0,0) = p.x - a.x;
	p1a.at<double>(0,1) = p.y-a.y;
	p1a.at<double>(0,2) = 1;

	Mat p2a = Mat::zeros(1,3,CV_64F);
	p2a.at<double>(0,0) = p2.x-a.x;
	p2a.at<double>(0,1) = p2.y-a.y;
	p2a.at<double>(0,2) = 1;

	Mat cp1 = ba.cross(p1a);
	Mat cp2 = ba.cross(p2a);

	double dotProduct = cp1.dot(cp2);

	return (dotProduct >=0);
}

//Scores the homography by translating points from real world into model and finding
//the cloest corresponding lines, then returns the score.
double ModelMatch::scoreHomography(vector<Point> rwPoints, vector<Point> modelPts, Mat H) {
	double score = 0;

	for(size_t i = 0; i < rwPoints.size(); i++) {
		Point p = rwPoints[i];
		Point pTestPt = translatePtoPPrime(p, H);
		double closestDist = minDist(modelPts, pTestPt);
		score += pow(closestDist, 2.0);
	}
	return score; //((double)score / rwPoints.size());
}

//Scores the homography by translating points from real world into model and finding
//the cloest corresponding lines, then returns the score.
double ModelMatch::scoreHomography(vector<Intersection> rwPoints, vector<Intersection> modelPts, Mat H) {
	double score = 0;

	for(size_t i = 0; i < rwPoints.size(); i++) {
		Point p = rwPoints[i].intSecPt;
		Point pTestPt = translatePtoPPrime(p, H);
		double closestDist = minDist(modelPts, pTestPt);
		score += closestDist;
		/*if(closestDist < 10) {
		score += closestDist;
		}
		else {
		score += 1000;
		}*/
	}
	return score; //((double)score / rwPoints.size());
}

//Translates all points in Model to points in Real World
vector<Point> ModelMatch::translateMtoRW(Mat H, vector<Point> modelPts) {
	vector<Point> translated;

	for(size_t m = 0; m < modelPts.size(); m++) {
		Point mPt = modelPts[m];
		Point rwPt = translatePPrimetoP(mPt, H);
		translated.push_back(rwPt);
	}
	return translated;
}

//Translates all points in Real World to Model
vector<Point> ModelMatch::translateRWtoM(Mat H, vector<Point> rwPts) {
	vector<Point> translated;

	for(size_t m = 0; m < rwPts.size(); m++) {
		Point rwPt = rwPts[m];
		Point mPt = translatePtoPPrime(rwPt, H);
		translated.push_back(mPt);
	}
	return translated;
}

//Translates point in model to frame
Point ModelMatch::translatePPrimetoP(Point p, Mat H) {

	Mat pVec = (Mat_<double>(3,1) << p.x, p.y, 1);
	Mat pPrime;
	solve(H, pVec, pPrime, CV_SVD);

	Point pPrimePt;

	double pZ = pPrime.at<double>(2,0);
	double pX = pPrime.at<double>(0,0) / pZ;
	double pY = pPrime.at<double>(1,0) / pZ;

	pPrimePt.x = (int)pX;
	pPrimePt.y = (int)pY;

	return pPrimePt;
}


//Translates point in frame to model
Point ModelMatch::translatePtoPPrime(Point pPrime, Mat H) {

	Mat pPrimeVec = (Mat_<double>(3,1) << pPrime.x, pPrime.y, 1);

	Mat p = H*pPrimeVec;
	Point pPt;
	double pZ = p.at<double>(2,0);
	double pX = p.at<double>(0,0) / pZ;
	double pY = p.at<double>(1,0) / pZ;

	pPt.x = (int)pX;
	pPt.y = (int)pY;

	return pPt;
}

//Computes the min distance from a translaed point to a corresponding set ot pts.
//Used to score homography.
double ModelMatch::minDist(vector<Point> pTargets, Point comp) {

	Point minPt = pTargets[0];
	double minDist = pow(minPt.x - comp.x, 2.0) + pow(minPt.y - comp.y, 2.0);

	for(size_t i = 1; i < pTargets.size(); i++) {
		Point iPt = pTargets[i];
		double dist = abs(iPt.x - comp.x + iPt.y - comp.y);
		if(dist < minDist) {
			minPt = iPt;
			minDist = dist;
		}
	}
	return minDist;
}

//Computes the min distance from a translaed point to a corresponding set ot pts.
//Used to score homography.
double ModelMatch::minDist(vector<Intersection> pTargets, Point comp) {

	Point minPt = pTargets[0].intSecPt;
	double minDist = pow(minPt.x - comp.x, 2.0) + pow(minPt.y - comp.y, 2.0);

	for(size_t i = 1; i < pTargets.size(); i++) {
		Point iPt = pTargets[i].intSecPt;
		//double dist = abs(iPt.x - comp.x + iPt.y - comp.y);
		double dist = pow(iPt.x - comp.x, 2.0) + pow(iPt.y - comp.y, 2.0);
		if(dist < minDist) {
			minPt = iPt;
			minDist = dist;
		}
	}
	return minDist;
}

//NOT USED. Randomly chooses two lines L1 and L2 s.t. L1 < L2
vector<Line> ModelMatch::chooseLinePair(vector<Line> lines) {
	unsigned int seed = (unsigned int)time(NULL);
	srand(seed);
	size_t lineSize = lines.size();

	int max = lineSize - 2;
	int min = 0;
	int randInd = rand();
	int leftChoice = (randInd%(max-min+1))+min;

	min = leftChoice+1;
	max = lineSize-1;
	randInd = rand();
	int rightChoice = (randInd%(max-min+1))+min;
	vector<Line> linePair;
	linePair.push_back(lines[leftChoice]);
	linePair.push_back(lines[rightChoice]);

	return linePair;
}

//Computes the 4 intersection points of 4 lines.
vector<Point> ModelMatch::computeIntersections(Line h1, Line h2, Line v1, Line v2) {

	Point p1 = h1.findIntersection(v1);
	Point p2 = h1.findIntersection(v2);
	Point p3 = h2.findIntersection(v1);
	Point p4 = h2.findIntersection(v2);

	vector<Point> pts;
	pts.push_back(p1);
	pts.push_back(p2);
	pts.push_back(p3);
	pts.push_back(p4);

	return pts;
}

//Computes the 4 intersection points of 4 lines.
vector<Point> ModelMatch::computeIntersections(Line h1, Line h2, Line v1, Line v2, vector<Intersection>& intsec) {

	Point p1 = h1.findIntersection(v1);
	Point p2 = h1.findIntersection(v2);
	Point p3 = h2.findIntersection(v1);
	Point p4 = h2.findIntersection(v2);

	vector<Point> pts;
	pts.push_back(p1);
	pts.push_back(p2);
	pts.push_back(p3);
	pts.push_back(p4);

	intsec = vector<Intersection>(4);

	intsec[0] = Intersection(p1);
	intsec[1] = Intersection(p2);
	intsec[2] = Intersection(p3);
	intsec[3] = Intersection(p4);

	return pts;
}

//Based on two sets of 4 intersection points, compute a homography matrix
Mat ModelMatch::computeHomography(vector<Point> rwIntSecPts, vector<Point> modelIntSecPts) {

	Point p1 = rwIntSecPts[0];
	Point p2 = rwIntSecPts[1];
	Point p3 = rwIntSecPts[2];
	Point p4 = rwIntSecPts[3];

	Point pP1 = modelIntSecPts[0];
	Point pP2 = modelIntSecPts[1];
	Point pP3 = modelIntSecPts[2];
	Point pP4 = modelIntSecPts[3];

	Mat pPrime = buildPPrime(pP1, pP2, pP3, pP4);

	Mat A = buildA(p1, p2, p3, p4,
		pP1, pP2, pP3, pP4);
	Mat HVec;
	solve(A, pPrime, HVec, CV_SVD);

	_H = Mat(3,3,CV_64F);

	_H.at<double>(0,0) = HVec.at<double>(0,0);
	_H.at<double>(0,1) = HVec.at<double>(1,0);
	_H.at<double>(0,2) = HVec.at<double>(2,0);
	_H.at<double>(1,0) = HVec.at<double>(3,0);
	_H.at<double>(1,1) = HVec.at<double>(4,0);
	_H.at<double>(1,2) = HVec.at<double>(5,0);
	_H.at<double>(2,0) = HVec.at<double>(6,0);
	_H.at<double>(2,1) = HVec.at<double>(7,0);
	_H.at<double>(2,2) = 1;

	return _H;
}

//helper function for computing homography without using opencv.
Mat ModelMatch::buildPPrime(Point p1, Point p2, Point p3, Point p4) {
	Mat pPrime = Mat::zeros(8,1,CV_64F);
	pPrime.at<double>(0,0) = p1.x;
	pPrime.at<double>(1,0) = -1*p1.y;
	pPrime.at<double>(2,0) = p2.x;
	pPrime.at<double>(3,0) = -1*p2.y;
	pPrime.at<double>(4,0) = p3.x;
	pPrime.at<double>(5,0) = -1*p3.y;
	pPrime.at<double>(6,0) = p4.x;
	pPrime.at<double>(7,0) = -1*p4.y;
	return pPrime;
}

//helper function for computing homography without using opencv.
Mat ModelMatch::buildA(Point p1, Point p2, Point p3, Point p4, 
	Point mp1, Point mp2, Point mp3, Point mp4) {

		Mat m = Mat::zeros(8,8,CV_64F);

		m.at<double>(0,0) = p1.x;
		m.at<double>(0,1) = p1.y;
		m.at<double>(0,6) = -1*p1.x*mp1.x;
		m.at<double>(0,7) =     -1*p1.y*mp1.x;
		m.at<double>(0,2) = 1;

		m.at<double>(1,3) = -1*p1.x;
		m.at<double>(1,4) = -1*p1.y;
		m.at<double>(1,6) = p1.x*mp1.y;
		m.at<double>(1,7) =     p1.y*mp1.y;
		m.at<double>(1,5) = -1;

		m.at<double>(2,0) = p2.x;
		m.at<double>(2,1) = p2.y;
		m.at<double>(2,6) = -1*p2.x*mp2.x;
		m.at<double>(2,7) =     -1*p2.y*mp2.x;
		m.at<double>(2,2) = 1;

		m.at<double>(3,3) = -1*p2.x;
		m.at<double>(3,4) = -1*p2.y;
		m.at<double>(3,6) = p2.x*mp2.y;
		m.at<double>(3,7) =     p2.y*mp2.y;
		m.at<double>(3,5) = -1;

		m.at<double>(4,0) = p3.x;
		m.at<double>(4,1) = p3.y;
		m.at<double>(4,6) = -1*p3.x*mp3.x;
		m.at<double>(4,7) =     -1*p3.y*mp3.x;
		m.at<double>(4,2) = 1;

		m.at<double>(5,3) = -1*p3.x;
		m.at<double>(5,4) = -1*p3.y;
		m.at<double>(5,6) = p3.x*mp3.y;
		m.at<double>(5,7) =     p3.y*mp3.y;
		m.at<double>(5,5) = -1;

		m.at<double>(6,0) = p4.x;
		m.at<double>(6,1) = p4.y;
		m.at<double>(6,6) = -1*p4.x*mp4.x;
		m.at<double>(6,7) =     -1*p4.y*mp4.x;
		m.at<double>(6,2) = 1;

		m.at<double>(7,3) = -1*p4.x;
		m.at<double>(7,4) = -1*p4.y;
		m.at<double>(7,6) = p4.x*mp4.y;
		m.at<double>(7,7) =     p4.y*mp4.y;
		m.at<double>(7,5) = -1;
		return m;
}

//NOT USED
double ModelMatch::getF(Mat homography) {
	Mat m = Mat::zeros(3,3,CV_64F);
	int width = 1280;
	int height = 720;
	m.at<double>(0,0) = 1;
	m.at<double>(1,1) = 1;
	m.at<double>(2,2) = 1;
	m.at<double>(0,2) = -1*(width/2);
	m.at<double>(1,2) = -1*(height/2);

	Mat HP = m*homography;

	double h00 = HP.at<double>(0,0);
	double h01 = HP.at<double>(0,1);
	double h10 = HP.at<double>(1,0);
	double h11 = HP.at<double>(1,1);
	double h20 = HP.at<double>(2,0);
	double h21 = HP.at<double>(2,1);

	double fsqr = (h00*h01 + h10*h11)/(h20*h21);
	double f = sqrt(fsqr);
	return f;
}

double ModelMatch::getB(Mat homography) {
	Mat m = Mat::zeros(3,3,CV_64F);
	int width = 1280;
	int height = 720;
	m.at<double>(0,0) = 1;
	m.at<double>(1,1) = 1;
	m.at<double>(2,2) = 1;
	m.at<double>(0,2) = -1*(width/2);
	m.at<double>(1,2) = -1*(height/2);

	Mat HP = m*homography;

	double h00 = HP.at<double>(0,0);
	double h01 = HP.at<double>(0,1);
	double h10 = HP.at<double>(1,0);
	double h11 = HP.at<double>(1,1);
	double h20 = HP.at<double>(2,0);
	double h21 = HP.at<double>(2,1);

	double fsqr = (h00*h01 + h10*h11)/(h20*h21);

	double h00sqr = pow(h00, 2.0);
	double h01sqr = pow(h01, 2.0);
	double h10sqr = pow(h10, 2.0);
	double h11sqr = pow(h11, 2.0);
	double h20sqr = pow(h20, 2.0);
	double h21sqr = pow(h21, 2.0);

	double Bsqr = (h01sqr + h11sqr + fsqr*h21sqr) / (h00sqr + h10sqr + fsqr*h20sqr);

	double B = sqrt(Bsqr);
	return B;
}

//Return the score of the best previous match attempt.
double ModelMatch::getScore() {
	return _score;
}

//NOT USED. Replaced with @scoreMappedModelLines2
double ModelMatch::scoreHomographyWithLineFilling(Mat lineMask, Mat H, 
	vector<Line> lines) {

		Mat newMask;
		lineMask.copyTo(newMask);

		for(size_t i = 0; i < lines.size(); i++) {
			Line newLine;
			Line& prevLine = lines[i];
			translateLine(H, newLine, prevLine);

			vector<Point> newPts = newLine.getPoints();
			line(lineMask, newPts[0], newPts[1], Scalar(0,0,0), 4);
		}

		return getBlackPixelScore(lineMask);
}

//NOT USED
double ModelMatch::getBlackPixelScore(Mat m) {

	double sum = 0;

	for(int r = 0; r < m.rows; r++) {
		for(int c = 0; c < m.cols; c++) {
			uchar value = m.at<uchar>(r,c);

			if(value == 0) { //if black
				sum += 1;
			}
			else if(value > 0) { //if white
				sum += -.5;
			}
		}
	}
	return sum;
}

//project a line using the given homography
void ModelMatch::translateLine(Mat H, Line& newLine, Line prevLine) {

	vector<Point> prevPts = prevLine.getPoints();
	Point p1 = prevPts[0];
	Point p2 = prevPts[1];

	Point newP1 = translatePPrimetoP(p1, H);
	Point newP2 = translatePPrimetoP(p2, H);

	newLine = Line(newP1, newP2);
}

cv::Mat ModelMatch::getHomography() {
	return _H;
}

//Draw the projected model line onto frame.
void ModelMatch::drawModelInFrame(Mat frame, Mat H, vector<Line>& mappedVLines, vector<Line>& mappedHLines) {
	vector<Line> vLines = _model.getVerLines();
	vector<Line> hLines = _model.getHorLines();
	for(size_t v = 0; v < vLines.size(); v++) {
		Line vl = vLines[v];
		Line newLine;
		translateLine(H, newLine, vl);
		newLine.drawLine(frame, Scalar(255,0,0));
		mappedVLines.push_back(newLine);
	}

	for(size_t h = 0; h < hLines.size(); h++) {
		Line hl = hLines[h];
		Line newLine;
		translateLine(H, newLine, hl);
		newLine.drawLine(frame, Scalar(255,0,0));
		mappedHLines.push_back(newLine);
	}
}

ModelMatch::~ModelMatch(void)
{
}